#include "WPILib.h"
#include "Robot2489.h"


void Robot2489::OperatorControl(void) {
	myRobot->SetSafetyEnabled(true);
	
	float left;
	float right;
	while (IsOperatorControl()) 	
	{
		left=leftStick->GetY();
		right=rightStick->GetY();
		leftMotor->Set(left);
		rightMotor->Set(right);
		manipulatorJag->Set(manipStick->GetY());
		
		
		onLowOutSwitch = lowOutSwitch->Get();
		onLowInSwitch = lowInSwitch->Get();
		onMidOutSwitch = midOutSwitch->Get();
		//onMidInSwitch = midInSwitch->Get();
		onMidInSwitch = midInSwitch->Get();
		onFeederSwitch = feederSwitch->Get();		
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "%d %lf", onMidOutSwitch, right);//"Height = %d", manipActualPeg);
			
		Manipulator();
		//myRobot->TankDrive(left,right);
		Wait(0.005); // wait for a motor update time
		/*screen->PrintfLine(DriverStationLCD::kUser_Line4, "C");
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "R");
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "M");*/
}
};
 
START_ROBOT_CLASS(Robot2489);
